#include<math.h>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Vector3Stamped.h>
#include<qpOASES.hpp>

using namespace std;

USING_NAMESPACE_QPOASES

#define g_control 9.8
#define pi 3.14
#define yaw 0
#define m_body 1.5

//框架大小 3*3*1
double x_l=-1.5;
double x_h=1.5;
double y_l=-1.5;
double y_h=1.5;
double z_l=1;
double z_h=2;

double testPos(geometry_msgs::PoseStamped pos)
{
    //int flag;
    double dist;
    double x=pos.pose.position.x;
    double y=pos.pose.position.y;
    double z=pos.pose.position.z;
    dist=min(x_h-x,x-x_l);
    dist=min(dist,y_h-y);
    dist=min(dist,y-y_l);
    dist=min(dist,z_h-z);
    dist=min(dist,z-z_l);
    
    return dist;
}

geometry_msgs::Vector3Stamped optimization(geometry_msgs::Vector3Stamped input,geometry_msgs::PoseStamped pose_,geometry_msgs::TwistStamped twst)
{
    
    geometry_msgs::Vector3Stamped output;
    real_t sol[3];//解出值

    //K值
    double k1=10;
    double k2=11;
    //飞机位置
    double pos_x=pose_.pose.position.x;
    double pos_y=pose_.pose.position.y;
    double pos_z=pose_.pose.position.z;

    //飞行速度
    double vel_x=twst.twist.linear.x;
    double vel_y=twst.twist.linear.y;
    double vel_z=twst.twist.linear.z;


    //ECBF约束条件

    real_t H[3*3]={2,0,0,
                                0,2,0,
                                0,0,2};
    //real_t g_cal[3]={(-2)*input.vector.x,(-2)*input.vector.y,(-2)*input.vector.z};
    real_t g_cal[3]={(-2)*input.vector.x,(-2)*input.vector.y,(-2)*input.vector.z};
    real_t A[6*3]=
        {1,0,0,
        -1,0,0,
        0,1,0,
        0,-1,0,
        0,0,1,
        0,0,-1};
    real_t ubA[6*1]={k1*(x_h-pos_x)+k2*(-vel_x),
                    k1*(pos_x-x_l)+k2*vel_x,
                    k1*(y_h-pos_y)+k2*(-vel_y),
                    k1*(pos_y-y_l)+k2*vel_y,
                    k1*(z_h-pos_z)+k2*(-vel_z),
                    k1*(pos_z-z_l)+k2*vel_z};
    QProblem flight(3,6);
    Options options;
	flight.setOptions( options );

    int_t nWSR=10;
    flight.init( H,g_cal,A,nullptr,nullptr,nullptr,ubA, nWSR);
    
    flight.getPrimalSolution(sol);

    output.vector.x=sol[0];
    output.vector.y=sol[1];
    output.vector.z=sol[2];
    ROS_INFO("ecbf_output=%f,%f,%f",sol[0],sol[1],sol[2]);


    return output;
}
